#ifndef __PATH_BASE_H__
#define __PATH_BASE_H__

#include <iostream>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include "core/common/core.h"
#include "core/utils/graphis_utils.h"
#include "core/utils/math_utils.h"
#include "core/convert/msg_convert.h"
#include "map_base.h"
#include "path_smooth.h"

using namespace std;


enum class PathGenMethod{
    A_STAR = 0,
    JPS = 1
};

class PlannerBase : public MapBase{
    public:
        PlannerBase();
        ~PlannerBase() = default;

        // 生成路径    
        mc::status genPath(geometry_msgs::Pose start, geometry_msgs::Pose goal, vector<geometry_msgs::Pose>& poses);
        // 虚函数
        virtual void genGridPath(int sx, int sy, int gx, int gy, vector<pair<int, int>>& points) = 0;

        // 平滑路径
        void pathSmooth(vector<geometry_msgs::Pose> origin_poses, vector<geometry_msgs::Pose>& smooth_poses);
        // 生成路径方向
        vector<geometry_msgs::Pose> genPathDirection(vector<geometry_msgs::Pose>& poses, 
            geometry_msgs::Pose goal);
        // 数值是否有效
        bool valueIsValid(int x, int y);
        bool valueIsValidByIdx(int index);
        bool valueIsValidForWorld(double wx, double wy);

        // 设置调试模式
        inline void setDebugMode(bool data){
            debug_ = data;
        }

    protected:
        bool debug_ = false;    //是否开启调试模式
    
    private:
        ros::NodeHandle nh_;
        ros::Publisher pathPub_;
        ros::Publisher pathOriginPub_;
        ros::Publisher startPosePub_;
        ros::Publisher goalPosePub_;

        string frameId_;
        PathSmoother path_smooth_;
};

#endif